package nx.behaviors;
import nx.robot.*;
import javaclient3.*;
import javaclient3.structures.*;
import nx.utils.*;


/**
 * Class implementing a Perception Schema (PS) for dealing with IR readings.
 *
 * This class converts IR readings into vector readings, for easy use with motor Schemas (MS).
 *
 * @author Alejandro Pustowka
 */
public class NxPSDetectObstaclesIR extends NxPSDetectObstacles {
    private final float MIN_RANGE=0.1f;
    private final float MAX_RANGE=1.0f;

    /**
     * @brief The IRInterface from Javaclient
     */
    IRInterface ir;


    Position2DInterface pos;

    Vector2D value=new Vector2D(0f,0f,Vector2D.MODE_XY);

    /**
     * Creates a Perception Schema based on IR readings.
     *
     * The output of this schema is an Array of Vector2D, each referenced to
     * the robots origin. That means, this schema is heavily based on localization
     * information. It has to be used with a Position2DInterface. In coming versions,
     * this could be fixed.
     * @param name Name of this Schema.
     * @param pos The Position2DInterface of Jaclient, from where the localization will be known.
     * @param ir The IRInterface of Javaclient, from where the readings come.
     */
    public NxPSDetectObstaclesIR(String name, Position2DInterface pos, PlayerDevice ir1) {

        super(name);

        //Assign Player Interfaces
        this.ir = (IRInterface)ir1;
        this.pos = pos;

        //query poses of the IRs
        ir.queryPose();

        //wait until geometry is ready
        while(!ir.isPoseReady());

        //get number of IRs, eq. to number of readings
        numReadings = ir.getPose().getPoses_count();

        //The poses of the IRs, it is only interesting the Yaw
        PlayerPose3d [] posesTemp;

        posesTemp = ir.getPose().getPoses();

        poseAngles = new double[numReadings];

        //get angle of poses (Yaw)
        for(int i=0;i<numReadings;i++)
        {
            poseAngles[i]=posesTemp[i].getPyaw();

            //print geometry of the sonar sensors
            System.out.println("x: "+Double.toString(posesTemp[i].getPx())+" y: "+Double.toString(posesTemp[i].getPy())+" yaw: "+Double.toString(Math.toDegrees(posesTemp[i].getPyaw())));
        }

        //create data length
        data = new Vector2D[poseAngles.length];

        //TODO: POR REVISAR QUE ES ESTO?????
        this.maxRange = 0.6f;
        this.minRange = 0.2f;
    }

     /**
     * Output of this Perception Schema (PS).
     * @return Array of Vector2D, one for each reading.
     */
    public Vector2D[] value() {

        //TODO: Esto deberia corregirse, no deberia ser bloqueante
        //TODO: Sin verificar isDataReady, se tendria lectura del anterior valor.
        // Sin embargo, la primera lectura si es necesario esperar hasta DataReady.
        //Hay que ver como colocar los comportamientos a modo de Hilos.
        while(!ir.isDataReady());

        //float [] dataTemp=ir.read(Sensor.READ_MODE_NO_WAIT);
        float [] dataTemp = ir.getData().getRanges();

        PlayerPose locPose = pos.getData().getPos();//.read(Sensor.READ_MODE_NO_WAIT);

        //as with the IRs we have cannot sense distance ranges on NXBot, set vector mag. as 1.
        for(int i=0;i<data.length;i++){

            data[i] = new Vector2D();
            
            if(dataTemp[i]<=minRange)
            {
                //if there is an obstacle, signal it. The pos of the obstacle
                //is with respect to origin (0,0)

                //define theta with respect to global axes
                data[i].setAngle((float)poseAngles[i]+ (float)locPose.getPa());
                //add the robot radius to the distance
                data[i].setMag(minRange + Robot.radius);
                //Translate vector from robot coordinates to global coordinates
                data[i] = data[i].add(new Vector2D((Vector2D)pos.getData().getPos()));


            }
                //if there is an obstacle, signal it in vector with zero
            else
                data[i].setMag(0);
            
            
        }
        return data;
    }
}
